package com.example.androidmaze;


public class StandardRobot implements Robot {
	
	
	/** In one direction */
	private static int senseCost = 1;
	/** Per 90deg turn */
	private static int rotateCost = 3;
	/** Per cell moved */
	private static int moveCost = 5;
	
	private boolean hasLeftSensor = true;
	private boolean hasRightSensor = true;
	private boolean hasFwdSensor = true;
	private boolean hasBckSensor = true;
	private boolean canMoveFwd = true;
	private boolean canMoveBck = true;
	private boolean canTurnLeft = true;
	private boolean canTurnRight = true;
	
	private int[] exitDirection;
	private int exitDistance = Integer.MAX_VALUE;
	
	private boolean obstacleHit = false;
	
	private int battery = 2500;
	
	
	private Maze maze;
	public int distTravelled = 0;
	
	public StandardRobot(Maze maze) {
		this.maze = maze;
	}
	
	
	@Override
	/**Method Name: rotate
	 * Input: Degree 90 for left and -90 for right
	 * 
	 * Process: Sets the current direction of the robot to the right
	 */
	public void rotate(int degree) throws UnsupportedArgumentException {
		assert Math.abs(degree) == (90): ("Degree is: " + degree);
		
		switch(degree)
		{
		case Constants.LEFT_ROTATE:
		{
			maze.keyDown('l');
			break;
		}
		case Constants.RIGHT_ROTATE:
		{
			maze.keyDown('r');
			break;
		}
		}
		
		
	}

	@Override
	public void move(int distance, boolean forward) throws HitObstacleException {
		obstacleHit = false;
		while (distance != 0 && obstacleHit == false) {//&& maze.state.getState() == Constants.STATE_PLAY) {
			//printCurrentInfo();
			if (forward) 
			{
				try {
					if (canSeeGoalAhead() && distance == 1 && exitDistance != Integer.MAX_VALUE)
					{
						maze.keyDown('s');
					}
					else
					{
						int [] nextPos = new int[2];
						nextPos[0] = getCurrentPosition()[0] + getCurrentDirection()[0];
						nextPos[1] = getCurrentPosition()[1] + getCurrentDirection()[1];
							if (hasWall(nextPos, getCurrentDirection())) {
								obstacleHit = true;
							}
								maze.keyDown('s');
							
						}
						//System.out.println(nextPos[0] + " " + nextPos[1]);
				}catch (UnsupportedMethodException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}
			else { //if backward
				try {
					if (canSeeGoalBehind() && distance == 1 && exitDistance != Integer.MAX_VALUE)
					{
						maze.keyDown('b');
					}
					else
					{
						int [] nextPos = new int[2];
						nextPos[0] = getCurrentPosition()[0] + getCurrentDirection()[0];
						nextPos[1] = getCurrentPosition()[1] + getCurrentDirection()[1];
							if (hasWall(nextPos, getCurrentDirection())) {
								obstacleHit = true;
							}
							maze.keyDown('b');
						}
						//System.out.println(nextPos[0] + " " + nextPos[1]);
				}catch (UnsupportedMethodException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
					}
			distTravelled ++;
			battery -= moveCost;
			distance --;	
		}
			

		if (obstacleHit) {
			throw new HitObstacleException(0);
		}
	}

	@Override
	public int[] getCurrentPosition() {
		int[] curPos = {maze.px, maze.py};
		return curPos;
	}

	@Override
	public boolean isAtGoal() {
		try {
			return canSeeGoalAhead();
		} catch (UnsupportedMethodException e) {
			e.printStackTrace();
			return false;
		}
	}

	@Override
	public int[] getCurrentDirection() {
		int[] curDir = maze.getDirection();
//		if (curDir !=  Constants.BOTTOM || curDir == Constants.TOP || curDir == Constants.LEFT || curDir == Constants.RIGHT)
//		{
//			System.out.println("getCurrentDirection is passing a valid Direction");
//		}
		return curDir;
	}

	@Override
	public float getCurrentBatteryLevel() {
		return battery;
	}
	
	@Override
	public float getEnergyForRotation() {
		return(rotateCost);
	}
	
	@Override
	public float getEnergyForFullRotation() {
		return (rotateCost * 4);
	}

	@Override
	public float getEnergyForStepForward() {
		return moveCost;
	}

	@Override
	public boolean hasStopped(){
		return (getCurrentBatteryLevel() <= 0 || obstacleHit) ;
		}

	@Override
	public boolean canSeeGoalAhead() throws UnsupportedMethodException {
		if (hasFwdSensor) {
			distanceToObstacleAhead();
			if (exitDistance != Integer.MAX_VALUE) { //if exit is found
				return true;
			}
			else { 
				return false;
			}
		}
		else {
			throw new UnsupportedMethodException();
		}
	}

	@Override
	public boolean canSeeGoalBehind() throws UnsupportedMethodException {
		if (hasBckSensor) {
			distanceToObstacleBehind();
			if (exitDistance != Integer.MAX_VALUE) { //if exit is found
				return true;
			}
			else {
				return false;
			}
		}
		else {
			throw new UnsupportedMethodException();
		}
	}

	@Override
	public boolean canSeeGoalOnLeft() throws UnsupportedMethodException {
		if (hasLeftSensor) {
			distanceToObstacleOnLeft();
			if (exitDistance != Integer.MAX_VALUE) { //if exit is found
				return true;
			}
			else {
				return false;
			}
		}
		else {
			throw new UnsupportedMethodException();
		}
	}

	@Override
	public boolean canSeeGoalOnRight() throws UnsupportedMethodException {
		if (hasRightSensor) {
			distanceToObstacleOnRight();
			if (exitDistance != Integer.MAX_VALUE) { //if exit is found
				return true;
			}
			else {
				return false;
			}
		}
		else {
			throw new UnsupportedMethodException();
		}
	}

	@Override
	public int distanceToObstacleAhead() throws UnsupportedMethodException {
		if (hasFwdSensor)
		{
			battery -= senseCost;
			return distToWall(getCurrentDirection());
		}
		else
		{
			throw new UnsupportedMethodException();
		}

	}

	@Override
	public int distanceToObstacleOnLeft() throws UnsupportedMethodException {
		if (hasLeftSensor) {
			int[] nextDir; //new direction that shifts current direction to LEFT
			if (getCurrentDirection() == Constants.TOP)
			{
				nextDir = Constants.LEFT;
			}
			else if (getCurrentDirection() == Constants.LEFT)
			{
				nextDir = Constants.BOTTOM;
			}
			else if (getCurrentDirection() == Constants.BOTTOM)
			{
				nextDir = Constants.RIGHT;
			}
			else if (getCurrentDirection() == Constants.RIGHT)
			{
				nextDir = Constants.TOP;
			}
			else
			{
				nextDir = new int[]{0,0};
			}
			battery -= senseCost;
			return distToWall(nextDir);
		}
		else {
			throw new UnsupportedMethodException();
		}
	}

	@Override
	public int distanceToObstacleOnRight() throws UnsupportedMethodException {
		if (hasRightSensor) {
			int[] nextDir; //new direction that shifts current direction to RIGHT
			if (getCurrentDirection() == Constants.RIGHT)
			{
				nextDir = Constants.BOTTOM;
			}
		else if (getCurrentDirection() == Constants.TOP)
		{
			nextDir = Constants.RIGHT;
		}
		else if (getCurrentDirection() == Constants.LEFT)
		{
			nextDir = Constants.TOP;
		}
		else if (getCurrentDirection() == Constants.BOTTOM)
		{
			nextDir = Constants.LEFT;
		}
		else
		{
			nextDir = new int[]{0,0};
			System.out.println("Invalid Direction");
		}
			battery -= senseCost;
			return distToWall(nextDir);
		}
		else {
			throw new UnsupportedMethodException();
		}
	}

	@Override
	public int distanceToObstacleBehind() throws UnsupportedMethodException {
		int[] nextDir;
		if (hasBckSensor) {
			
			if (getCurrentDirection() == Constants.RIGHT)
				{
					nextDir = Constants.LEFT;
				}
			else if (getCurrentDirection() == Constants.TOP)
			{
				nextDir = Constants.BOTTOM;
			}
			else if (getCurrentDirection() == Constants.LEFT)
			{
				nextDir = Constants.RIGHT;
			}
			else if (getCurrentDirection() == Constants.BOTTOM)
			{
				nextDir = Constants.TOP;
			}
			else
			{
				nextDir = new int[]{0,0};
			}
			
//			int[] nextDir; //new direction that shifts current direction to BEHIND
//			nextDir = new int[] { (-1 * getCurrentDirection()[0]), (-1 * getCurrentDirection()[1])};
//			System.out.println("Behind nextDir:" + nextDir[0] + ", " +nextDir[1]);
//			System.out.println("Behind Get Direction: "+ getCurrentDirection()[0]+ ", " + getCurrentDirection()[1]);
			battery -= senseCost;
			return distToWall(nextDir);
		}
		else {
			throw new UnsupportedMethodException();
		}
	}
	
	
	/**Inputs: Maze, Current Position of Robot, Direction to sense 
	 * Output: Distance to the nearest wall in that direction
	 * */
	protected int distToWall(int[] direction){
		int[] curPos = getCurrentPosition();
		int boundaryDistance = getBoundaryDistance(direction);
		
		int distanceToWall = 0;
		while (distanceToWall < boundaryDistance && !hasWall(curPos, direction)) {
			curPos[0] += direction[0];
			curPos[1] += direction[1];
			distanceToWall ++;
			}
		//System.out.println("Direction:  "+direction[0]+", "+ direction[1]);
		//System.out.println("Boundary Distance: " + boundaryDistance );
		if (distanceToWall ==  boundaryDistance && !hasWall(curPos,direction))
		{
			//System.out.println("Exit Values Set");
			exitDistance = distanceToWall;
			exitDirection = direction;
			//This is the exit
		}
		else {
			exitDistance = Integer.MAX_VALUE;
		}
		return distanceToWall;
		
	}
	
	protected boolean hasWall(int[] curPos, int[] direction) {
		//System.out.println("direction == Constants.LEFT: " + (direction == Constants.LEFT));
		if (direction == Constants.LEFT) {
			return maze.mazecells.hasWallOnLeft(curPos[0], curPos[1]);
		}
		else if (direction == Constants.RIGHT){
			return maze.mazecells.hasWallOnRight(curPos[0], curPos[1]);
		}
		else if (direction == Constants.TOP) {
			return maze.mazecells.hasWallOnTop(curPos[0], curPos[1]);
		}
		/* Assume other direction must be bottom */
		else  {
			assert(direction == Constants.BOTTOM): "Invalid Direction received (not left/right/top/bottom)";
			return maze.mazecells.hasWallOnBottom(curPos[0], curPos[1]);
		}
	}
/**
 * 
 * @param direction
 * @return
 */
	
	protected int getBoundaryDistance(int[] direction)
	{
		int[] curPos = getCurrentPosition();
		int boundary_distance = 0;
//		assert(direction == Constants.LEFT || direction == Constants.RIGHT || direction == Constants.BOTTOM || direction == Constants.TOP): "Invalid Direction received";
		if (direction == Constants.LEFT) {
			boundary_distance = curPos[0];
		}
		else if (direction == Constants.RIGHT) {
			boundary_distance = maze.getWidth()- 1 - curPos[0];
		}
		else if (direction == Constants.BOTTOM) {
			boundary_distance = maze.getHeight() - 1 -  curPos[1];
		}
		else if (direction == Constants.TOP) {
			boundary_distance = curPos[1];
		}
		else
		{
			System.out.println("Invalid Direction Passed");
		}
		//System.out.println("Direction: " + direction[0] + ", "+ direction[1]+"\nBoundary Distance: "+ boundary_distance );
		return boundary_distance;
	}
	
	public void printCurrentInfo()
	{
		System.out.println("\nDisplaying Current Robot Information\n ================================");
		System.out.println("Current Position: " + getCurrentPosition()[0] + ", "+ getCurrentPosition()[1]);
		System.out.println("Current Direction: " + getCurrentDirection()[0] + ", " + getCurrentDirection()[1]);
		try {
			System.out.println("Distance to Forward Wall: " + distanceToObstacleAhead());
		} catch (UnsupportedMethodException e) {
			e.printStackTrace();
		}
		try {
			System.out.println("Distance to Left Wall: " + distanceToObstacleOnLeft());
		} catch (UnsupportedMethodException e) {
			e.printStackTrace();
		}
		try {
			System.out.println("Distance to Behind Wall: " + distanceToObstacleBehind());
		} catch (UnsupportedMethodException e) {
			e.printStackTrace();
		}
		try {
			System.out.println("Distance to Right Wall: " + distanceToObstacleOnRight());
		} catch (UnsupportedMethodException e) {
			e.printStackTrace();
		}
		
	}
	public int getDistTravelled() {
		return distTravelled;
	}
	
	public void escape() {
		maze.keyDown('e');
	}
	
	public void canSeeGoal(int[] direction)
	{
		
	}
	
	public int getDistTraveled()
	{
		return distTravelled;
	}
	public int getExitDistance()
	{
		return exitDistance;
	}
	
	public void decreaseBatteryLevel(float amount) {
		battery -= amount;
	}
	public int getMazeWidth()
	{
		return maze.getWidth();
	}

	
	
}
